开发者说丨Apollo代码学习—模型预测控制（MPC)

知识点

敲黑板，本文需要学习的知识点有

MPC（模型预测控制）是一种先进的过程控制方法，在满足一定约束条件的前提下，被用来实现过程控制，它的实现依赖于过程的动态模型（通常为线性模型）。

在控制时域（一段有限时间）内，它主要针对当前时刻进行优化，但也考虑未来时刻，求取当前时刻的最优控制解，然后反复优化，从而实现整个时域的优化求解。

Apollo中用到了PID、MPC和LQR三种控制器，其中，MPC(Model Predictive Control，模型预测控制)和LQR(Linear–Quadratic Regulator，线性二次调解器) 在状态方程、控制实现等方面，有很多相似之处，但也有很多不同之处，如工作时域、最优解等。

本文由社区开发者-吕伊鹏撰写，对Apollo代码学习—模型预测控制（MPC)进行了详细讲解，希望这篇文给感兴趣的同学带来更多帮助。

以下，ENJOY

查看Apollo中关于MPC_controller的代码可以发现，它的主体集成了横纵向控制，在计算控制命令时，计算了横纵向误差：

1ComputeLongitudinalErrors(&trajectory_analyzer_, debug);
2
3ComputeLateralErrors(com.x(), com.y(),
4                   VehicleStateProvider::instance()->heading(),
5                   VehicleStateProvider::instance()->linear_velocity(),
6                   VehicleStateProvider::instance()->angular_velocity(),
7                   trajectory_analyzer_, debug);

下文将从MPC原理入手，结合横纵向控制进行分析。

可以先结合官方的教学视频对MPC进行一个简单的了解：模型预测控制。

根据维基百科，模型预测控制是一种先进的过程控制方法，在满足一定约束条件的前提下，被用来实现过程控制，它的实现依赖于过程的动态模型（通常为线性模型）。

在控制时域（一段有限时间）内，它主要针对当前时刻进行优化，但也考虑未来时刻，求取当前时刻的最优控制解，然后反复优化，从而实现整个时域的优化求解。

也就是说，模型预测控制实际上是一种时间相关的，利用系统当前状态和当前的控制量，来实现对系统未来状态的控制。而系统未来的状态是不定的，因此在控制过程中要不断根据系统状态对未来的控制量作出调整。

而且相较于经典的的PID控制，它具有优化和预测的能力，也就是说，模型预测控制是一种致力于将更长时间跨度、甚至于无穷时间的最优化控制问题，分解为若干个更短时间跨度，或者有限时间跨度的最优化控制问题，并且在一定程度上仍然追求最优解1。

可以通过下图对模型预测控制进行一个简单的理解：

图1 模型预测控制原理

图1中，k轴为当前状态，左侧为过去状态，右侧为将来状态。可结合无人驾驶车辆模型预测控制2第3.1.2节来理解此图。

模型预测控制在实现上有三个要素：

1. 预测模型，是模型预测控制的基础，用来预测系统未来的输出；

2. 滚动优化，一种在线优化，用于优化短时间内的控制输入，以尽可能减小预测模型输出与参考值的差距；

3. 反馈矫正，在新的采样时刻，基于被控对象的实际输出，对预测模型的输出进行矫正，然后进行新的优化，以防模型失配或外界干扰导致的控制输出与期望差距过大。

下面从三要素入手对模型预测控制进行分析。

无论是运动学模型，还是动力学模型，所搭建的均为非线性系统，而线性模型预测控制较非线性模型预测控制有更好的实时性，且更易于分析和计算。对于无人车来说，实时性显然很重要，因此，需要将非线性系统转化为线性系统，而非线性系统的线性化的方法大体可分为精确线性化和近似线性化，多采用近似的线性化方法。

非线性系统线性化的方法有很多种，大致分为：

图2 线性化方法

下面以泰勒展开的方法为例，结合《无人驾驶车辆模型预测控制》23.3.2节非线性系统线性化方法，展示一种存在参考系统的线性化方法。存在参考系统的线性化方法假设参考系统已经在规划轨迹上完全跑通，得出每个时刻上相应的状态量和控制量，然后通过处理参考系统与当前系统的偏差，利用模型预测控制器来跟踪期望路径。

假设车辆参考系统在任意时刻的状态和控制量满足如下方程：

在任意点处泰勒展开，只保留一阶项，忽略高阶项，有：

公式2与公式1相减可得：

其中，，为对的雅克比矩阵,为对的雅克比矩阵。此时，通过雅克比矩阵，将非线性系统近似转化为一个连续的线性系统，但并不适于模型预测控制器的设计，然后对其离散化处理可得，

得到线性化时变模型：

其中：，为单位矩阵。

至此，得到一个非线性系统在任意一参考点线性化后的线性系统，该系统是设计模型预测控制算法的基础。

以运动学模型中的式1.15为例，低速情况下的车辆运动学模型可表达为：

其状态变量和控制变量分别为：

对应的雅克比矩阵：

其线性时变模型为：

其中，

预测模型仍以单车模型为主，结合运动学模型和动力学模型对车辆运动状态进行分析。

图3 单车模型

根据化代码可知，Apollo的MPC模块中的状态变量共有6个：

1matrix_state_ = Matrix::Zero(basic_state_size_, 1);
2
3// State matrix update;
4matrix_state_(0, 0) = debug->lateral_error();
5matrix_state_(1, 0) = debug->lateral_error_rate();
6matrix_state_(2, 0) = debug->heading_error();
7matrix_state_(3, 0) = debug->heading_error_rate();
8matrix_state_(4, 0) = debug->station_error();
9matrix_state_(5, 0) = debug->speed_error();

它们的计算请参考上一篇Apollo代码学习(五)—横纵向控制：

控制变量有2个：

1Eigen::MatrixXd control_matrix(controls_, 1);
2control_matrix << 0, 0;

并结合代码去分析control_matrix内包含的变量：

 1// 解mpc，输出一个vector，control内有10个control_matrix
 2SolveLinearMPC(matrix_ad_, matrix_bd_, matrix_cd_, matrix_q_updated_,
 3      matrix_r_updated_, lower_bound, upper_bound, matrix_state_, reference,
 4      mpc_eps_, mpc_max_iteration_, &control)
 5// 已知，mpc仅取第一组解为当前时刻最优控制解，即control[0]
 6// control中第一个矩阵中的第一个值用于计算方向盘转角
 7double steer_angle_feedback = control[0](0, 0) * 180 / M_PI *
 8                            steer_transmission_ratio_ /
 9                            steer_single_direction_max_degree_ * 100;
10double steer_angle = steer_angle_feedback + steer_angle_feedforwardterm_updated_;
11// control中第一个矩阵中的第二个值用于计算加速度
12debug->set_acceleration_cmd_closeloop(control[0](1, 0));
13double acceleration_cmd = control[0](1, 0) + debug->acceleration_reference();

可得

其中，为前轮转角，为加速度补偿。

结合方向盘控制的动力学模型：

和mpc_controller.cc、mpc_slover.cc中的代码注释。

1// 初始化矩阵
2Status MPCController::Init(const ControlConf *control_conf) {
3...
4// Matrix init operations.
5matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
6matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
7...
8// TODO(QiL): expand the model to accomendate more combined states.
9
10matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
11...
12
13matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
14matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
15...
16matrix_bd_ = matrix_b_ * ts_;
17
18matrix_c_ = Matrix::Zero(basic_state_size_, 1);
19...
20matrix_cd_ = Matrix::Zero(basic_state_size_, 1);
21...
22}
23
24// 更新矩阵
25void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
26const double v = VehicleStateProvider::instance()->linear_velocity();
27matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
28matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
29matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
30matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
31
32Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
33matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *
34           (matrix_i - ts_ * 0.5 * matrix_a_).inverse();
35
36matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;
37matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;
38matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;
39}
40
41// 求解MPC
42// discrete linear predictive control solver, with control format
43// x(i + 1) = A * x(i) + B * u (i) + C
44bool SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b,
45                const Matrix &matrix_c, const Matrix &matrix_q,
46                const Matrix &matrix_r, const Matrix &matrix_lower,
47                const Matrix &matrix_upper,
48                const Matrix &matrix_initial_state,
49                const std::vector &reference, const double eps,
50                const int max_iter, std::vector *control) 


可得MPC控制模型：

其中，分别为前/后轮转弯刚度，分别为前悬/后悬长度，m为车身质量，V为车速，为车辆绕z轴转动的转动惯量，为转向速度。

对应的状态矩阵、控制矩阵、扰动矩阵等如下：

对应的线性化系数应为（欧拉映射离散法）：

但代码中实际线性化系数为（双线性变换离散法）：

1 matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *
2           (matrix_i - ts_ * 0.5 * matrix_a_).inverse();
3
4matrix_bd_ = matrix_b_ * ts_;
5
6matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;

对于的形式不同，个人认为是由于heading_error_rate是计算横向误差之后才有的，无法直接获取，在矩阵更新的时候才将heading_error_rate作为系数更新到矩阵中，也就是公式10中的其实对于仍是的形式。的形式不一样，是因为此处采用了双线性变换离散法。感谢博友LLCCCJJ的友情提示。 关于连续系统离散化的方法可参考：连续系统离散化方法。

最终得到形如公式13的离散线性模型：

系统的输出方程为：

则在预测时域内有状态变量方程和输出变量方程如下：

假设

可得到新的状态空间表达式：

其中，

可将公式16、公式18代入公式17，验证公式18的由来。

则在预测时域内的状态变量和输出变量可用如下算式计算：

将系统未来时刻的状态和输出以矩阵形式表达为：

其中，

通过公式20我们可以看出，预测时域内的状态和输出量都可以通过从系统当前的状态量和控制增量求得，这也即是模型预测控制算法中“预测”功能的实现。

结合mpc_slover.cc，对MPC的模型进行分析：

1unsigned int horizon = reference.size();  // horizon =10
2
3// Update augment reference matrix_t
4// matrix_t为60*1的矩阵，存储的参考轨迹信息
5Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1);
6...
7
8// Update augment control matrix_v
9// matrix_v为20*1的矩阵，存储控制信息
10Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1);
11...
12
13// matrix_a_power为含有10个矩阵的容器，每个矩阵为6*6，matrix_a为6*6矩阵
14std::vector matrix_a_power(horizon);
15...
16
17// matrix_k为60*20的矩阵，matrix_b为6*2矩阵
18Matrix matrix_k =
19   Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * horizon);
20...  


令根据代码有：

其中，为参考轨迹序列，为预测控制序列，预测时域为10个采样周期。

滚动优化的目的是为了求最优控制解，是一种在线优化，通过使某一或某些性能指标达到最优来实现控制作用。换句话说，就是可以使车辆以相对最少的操作（或损耗代价）获得相对最好的轨迹跟踪（或控制）效果。

因此，需要设计合适的优化目标，以获得尽可能最优的控制序列，目标函数的一般形式可表示为状态和控制输入的二次函数：

其中，N为预测时域，Q,R为权重矩阵，且满足的正定矩阵。形如表示，在n时刻下预测的m时刻的值，第一项反映了系统对参考轨迹的跟踪能力，第二项反映了对控制量变化的约束。此外，上式（公式23）在书写过程中，累加和函数后几乎都不加"()"，但我看起来实在难受，所以自行将x，u一起括起来。

优化求解的问题可以理解为在每一个采样点k，寻找一组最优控制序列和最优开销,其中:

在MPC控制规律中，将控制序列中的第一个参数作为控制量，输入给被控系统。

对于车辆控制来说，就是找到一组合适的控制量，如，其中，为前轮偏角，为刹车/油门系数，使车辆沿参考轨迹行驶，且误差最小、控制最平稳、舒适度最高等。因此在设计目标函数的时候可以考虑以下几点：

1. 看过Apollo官方控制模块视频的人，可能知道在设计代价函数时候，一般设计为二次型的样式，为的是避免在预测时域内，误差忽正忽负，导致误差相互抵消；此外，对于实在不理解为什么代价函数要采用平方形式的同学，也可以参考损失函数为什么用平方形式（二）。

2. 可考虑的代价有：

   a.距离误差(Cross Track Error, CTE)，指实际轨迹点与参考轨迹点间的距离，误差项可表示为：；

   b.速度误差，指实际速度与期望速度的差，误差项可表示为：；

   c.刹车/油门调节量，目的是为了保证刹车/油门变化的平稳性，误差项可表示为：；

   d.航向误差等…

3. 约束条件

   
   a. 最大前轮转角
   b. 最大刹车/油门调节量
   c. 最大方向盘转角
   d. 最大车速等

因此公式23中的第一项可表示为：

其中，为该项误差的权重，对应权重矩阵Q中的某一项。

对于上述的目标函数（公式23），是有多个变量，且要服从于这些变量的线性约束的二次函数，因此可以通过适当处理将其转换为二次规划问题。

基于公式20，将控制量变为控制增量，以满足系统对控制增量的要求，则目标函数可以改写为：

在此基础上满足一些约束条件，一般如下：

其中， 。

将公式20代入公式23中，并将预测时域内输出量偏差表示为：

其中，

经过一定的矩阵计算，可将目标函数调整为与控制增量相关的函数：

其中，                                                                                        ，为常量，为权重矩阵的扩充矩阵。

结合mpc_slover.cc中的代码，

1// Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr,
2// vector of matrix A power
3Matrix matrix_m = Matrix::Zero(matrix_b.rows() * horizon, 1);      // 60*1
4Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows()); // 60*60
5Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols()); // 20*20
6Matrix matrix_ll = Matrix::Zero(horizon * matrix_lower.rows(), 1); //20*1
7Matrix matrix_uu = Matrix::Zero(horizon * matrix_upper.rows(), 1); //20*1
8...
9// Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
10Matrix matrix_m1 = matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr;
11Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t);
12...
13// TODO(QiL) : change qp solver to box constraint or substitute QPOASES
14// Method 1: QPOASES
15Matrix matrix_inequality_constrain_ll =
16  Matrix::Identity(matrix_ll.rows(), matrix_ll.rows());
17...
18// 求解
19std::unique_ptr qp_solver(new ActiveSetQpSolver(
20  matrix_m1, matrix_m2, matrix_inequality_constrain,
21  matrix_inequality_boundary, matrix_equality_constrain,
22  matrix_equality_boundary));
23auto result = qp_solver->Solve();


令，根据代码有：

因此，对模型预测控制的每一步进行带约束条件的优化问题，可转换为如下的二次规划问题：

此外，在mpc_slover.cc中有注释如下：

1// Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
2
3// Format in qp_solver
4/**
5* *           min_x  : q(x) = 0.5 * x^T * Q * x  + x^T c
6* *           with respect to:  A * x = b (equality constraint)
7* *                             C * x >= d (inequality constraint)
8* **/
9// TODO(QiL) : change qp solver to box constraint or substitute QPOASES
10// Method 1: QPOASES


可知，Apollo将MPC的优化求解转化为二次规划(Quadratic Programming, QP)问题，且QP问题的求解采用QPOASES的方法，QPOASES方法详解：

，可以结合active_set_qp_solver.cc以及矩阵计算库Eigen对QPOASES方法进行理解分析。

在每个控制周期内，按照公式30完成优化求解后，得到控制时域内的一系列控制增量：

将序列中的第一个控制增量作为实际的控制输入增量作用于系统，则有：

将作为此时的控制量输入给系统，直到下一个控制时刻，系统根据新的状态信息预测下一时段内的输出，然后通过优化得到一组新的控制序列。如此反复，直至完成整个控制过程。

综上所述，MPC控制器的工作原理大致如下所示：

图4 MPC控制原理框图

其中，为t时刻车辆的观测状态，为t时刻车辆的估计状态，为t时刻的最优控制解，为t时刻的系统输出。

模型预测控制的实现，依赖MPC控制器、被控车辆、状态估计器、轨迹规划等信息。结合图1和图4，模型预测控制器的一般工作步骤可以概括如下：

1、在t时刻，结合历史信息和当前状态以及预测模型，预测N步的系统输出；

2、结合约束条件等，设计目标函数，计算最优控制解，输入到被控车辆，使其在当前控制量下运动；

3、获取车辆状态输入到状态估计器中，对那些无法直接用传感器获取或观测成本较高的状态量进行估计，然后将输入到MPC控制器，再次进行优化求解，以得到未来一段时间的预测控制序列；

4、然后在t+1时刻重复上述步骤，如此，滚动地实现带约束的优化问题，从而实现对被控对象的连续控制。

此外，也可在需要的时候，被用来更新规划轨迹，以便得到更好的控制效果。

* 本篇内容中的状态方程基于增量输出delta_u，建议开发者使用Apollo的写法（基于绝对输出u），以保证上车验证效果。

特别感谢百度研发工程师-崔霄的技术支持！

* 以上内容为开发者原创，不代表百度官方言论。

  已获开发者授权，原文地址请戳阅读原文。